#include "arm_pkg/arm.h"
#include "nav_pkg/Navigate.h"
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("behavior_tree_robot");
    BT::BehaviorTreeFactory factory;
    factory.registerNodeType<Navigate>("Navigate");
    factory.registerNodeType<Arm>("Arm");
    auto tree = factory.createTreeFromFile("./mytree.xml");
    while (rclcpp::ok() && tree.rootNode()->status() != BT::NodeStatus::FAILURE)
    {
        tree.tickRoot();
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(100));
    }
    rclcpp::shutdown();
    return 0;
}
